
#include <BMesh>
#include <BSphere>
#include <BConvexHull>
#include "ISoftBodyConcaveAlgorithm.h"
#include "ITriangleShape.h"
#include "ISimplexConvexCast.h"
#include "member_BBody.h"
#include "member_BNode.h"

#define BT_SOFTBODY_TRIANGLE_EXTRUSION BReal(0.06)

inline void TransformSpace(const BVector& halfExtents, BReal margin, const BMatrix& t, BVector& aabbMinOut, BVector& aabbMaxOut)
{
	BVector halfExtentsWithMargin = halfExtents + BVector(margin, margin, margin);
	BMatrix abs_b = t.absolute();
	BVector center = t.position();
	BReal ex = halfExtentsWithMargin.dot(abs_b.column(0));
	BReal ey = halfExtentsWithMargin.dot(abs_b.column(1));
	BReal ez = halfExtentsWithMargin.dot(abs_b.column(2));
	BVector extent(ex, ey, ez);
	aabbMinOut = center - extent;
	aabbMaxOut = center + extent;
}

inline void TransformSpace(const BVector& localAabbMin, const BVector& localAabbMax, BReal margin, const BMatrix& trans, BVector& aabbMinOut, BVector& aabbMaxOut)
{
	BVector localHalfExtents = BReal(0.5) * (localAabbMax - localAabbMin);
	localHalfExtents += BVector(margin, margin, margin);

	BVector localCenter = BReal(0.5) * (localAabbMax + localAabbMin);
	BMatrix abs_b = trans.absolute();
	BVector center = trans * localCenter;
	BReal ex = localHalfExtents.dot(abs_b.column(0));
	BReal ey = localHalfExtents.dot(abs_b.column(1));
	BReal ez = localHalfExtents.dot(abs_b.column(2));
	BVector extent(ex, ey, ez);
	aabbMinOut = center - extent;
	aabbMaxOut = center + extent;
}

ISoftBodyConcaveAlgorithm::ISoftBodyConcaveAlgorithm()
{
	m_softBodyTriangleCallback.m_softBody = (BSoftBody*)_bodyA;
	m_softBodyTriangleCallback.m_triBody = _bodyB;
}

ISoftBodyConcaveAlgorithm::~ISoftBodyConcaveAlgorithm()
{

}

void SoftBodyTriangleCallback::processTriangle()
{

}

void SoftBodyTriangleCallback::setTimeStepAndCounters(BMesh* mesh, BReal collisionMarginTriangle, const BBody* triBodyWrap)
{
	m_collisionMarginTriangle = collisionMarginTriangle + BReal(BT_SOFTBODY_TRIANGLE_EXTRUSION);

	BVector aabbWorldSpaceMin, aabbWorldSpaceMax;
	m_softBody->getAabb(aabbWorldSpaceMin, aabbWorldSpaceMax);
	BVector halfExtents = (aabbWorldSpaceMax - aabbWorldSpaceMin) * BReal(0.5);
	BVector softBodyCenter = (aabbWorldSpaceMax + aabbWorldSpaceMin) * BReal(0.5);

	BMatrix softTransform(softBodyCenter);

	BMatrix convexInTriangleSpace;
	convexInTriangleSpace = node_member(triBodyWrap)->matrix.inverse() * softTransform;
	TransformSpace(halfExtents, m_collisionMarginTriangle, convexInTriangleSpace, m_aabbMin, m_aabbMax);
}

void ISoftBodyConcaveAlgorithm::caculate()
{
	BBody* convexBody = _bodyA;
	const BBody* triBody = _bodyB;
	if (BMesh* mesh = dynamic_cast<BMesh*>(_shapeB))
	{
		BReal collisionMarginTriangle = mesh->margin();
		m_softBodyTriangleCallback.setTimeStepAndCounters(mesh, collisionMarginTriangle, triBody);
		BSpace space(m_softBodyTriangleCallback.getAabbMin(), m_softBodyTriangleCallback.getAabbMax());
		m_softBodyTriangleCallback.processTriangle();
	}
}

void ISoftBodyConcaveAlgorithm::timeImpact()
{
	BBody* convexbody = _bodyA;
	BBody* triBody = _bodyB;

	const BMatrix& matrixA = node_member(convexbody)->matrix;
	const BMatrix& matrixB = node_member(triBody)->matrix;

	const BMatrix& interpolationWorldTransformA = body_member(convexbody)->predictedMatrix;
	const BMatrix& interpolationWorldTransformB = body_member(triBody)->predictedMatrix;

	BMatrix triInv = matrixB.inverse();
	BMatrix source = triInv * matrixA;
	BMatrix target = triInv * interpolationWorldTransformA;

	struct LocalTriangleSphereCastCallback
	{
		BMesh* m_mesh;
		BMatrix m_ccdSphereFromTrans;
		BMatrix m_ccdSphereToTrans;
		BMatrix m_meshTransform;

		BReal m_ccdSphereRadius;
		BReal m_hitFraction;

		LocalTriangleSphereCastCallback(BMesh* mesh, const BMatrix& from, const BMatrix& to, BReal ccdSphereRadius, BReal hitFraction)
		{
			m_mesh = mesh;
			m_ccdSphereFromTrans = from;
			m_ccdSphereToTrans = to;
			m_ccdSphereRadius = ccdSphereRadius;
			m_hitFraction = hitFraction;
		}

		virtual void processTriangle(const BSpace& space)
		{
			BVector* triangle = 0;

			BMatrix ident;
			CastResult caseResult;
			caseResult.fraction = m_hitFraction;
			BSphere pointShape(m_ccdSphereRadius);
			ITriangleShape triShape(triangle[0], triangle[1], triangle[2]);
			IVoronoiSimplexSolver simplexSolver;
			ISimplexConvexCast convexCaster(&pointShape, &triShape, &simplexSolver);

			convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans, m_ccdSphereToTrans, ident, ident, caseResult);
			if (m_hitFraction > caseResult.fraction)
				m_hitFraction = caseResult.fraction;
		}
	};

	if (BMesh* mesh = dynamic_cast<BMesh*>(_shapeB))
	{
		BVector rayAabbMin = source.position();
		SetMin(rayAabbMin, target.position());
		BVector rayAabbMax = source.position();
		SetMax(rayAabbMax, target.position());
		BReal ccdRadius0 = convexbody->box().radius();
		rayAabbMin -= BVector(ccdRadius0, ccdRadius0, ccdRadius0);
		rayAabbMax += BVector(ccdRadius0, ccdRadius0, ccdRadius0);

		BReal curHitFraction = BReal(1.);
		LocalTriangleSphereCastCallback raycastCallback(mesh, source, target, ccdRadius0, curHitFraction);

		raycastCallback.m_hitFraction = body_member(convexbody)->solverData.hitFraction;

		BBody* concavebody = triBody;

		raycastCallback.processTriangle(BSpace(rayAabbMin, rayAabbMax));

		if (raycastCallback.m_hitFraction < body_member(convexbody)->solverData.hitFraction)
		{
			body_member(convexbody)->solverData.hitFraction = raycastCallback.m_hitFraction;
		}
	}

}
